﻿// The MIT License (MIT)
// Copyright (c) 2013 Stylianos Dritsas
//
// Permission is hereby granted, free of charge, to any person obtaining a 
// copy of this software and associated documentation files (the "Software"), 
// to deal in the Software without restriction, including without limitation 
// the rights to use, copy, modify, merge, publish, distribute, sublicense, 
// and/or sell copies of the Software, and to permit persons to whom the 
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included 
// in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
// DEALINGS IN THE SOFTWARE.


//-- System Imports
//--
using System;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.Collections.Generic;

//-- Library Imports
//--
using Rhino.Geometry;
using Rhino.Display;
using BulletSharp;

//-- Disambiguation / Aliases
//--
using Mat4D    = BulletSharp.Matrix;
using Vec3D    = BulletSharp.Vector3;
using World    = BulletSharp.DynamicsWorld;
using Phase    = BulletSharp.BroadphaseInterface;
using Config   = BulletSharp.CollisionConfiguration;
using Shapes   = BulletSharp.AlignedCollisionShapeArray;
using Dispatch = BulletSharp.CollisionDispatcher;
using Solver   = BulletSharp.SequentialImpulseConstraintSolver;

namespace Sutd
{
  public partial class Command
  {
    public static class Physics
    {
      private static Dialog   dialog;        //-- Control Panel
      private static Thread   thread;        //-- Animation Thread
      private static Conduit  conduit;       //-- Display Conduit
      private static Config   config;        //-- Collision Configuration
      private static Dispatch dispatch;      //-- Collision Dispatcher
      private static Phase    phase;         //-- Collision Broadphase
      private static Solver   solver;        //-- Constraint Solver
      private static World    world;         //-- Collision World
      private static Shapes   shapes;        //-- Collision Shapes
      private static Bodies   bodies;        //-- Bullet/Rhino Bridge

      #region -- Display Conduit ----------------------------------------------

      //-- Dynamic Viewport Drawing 
      //--
      public class Conduit : Rhino.Display.DisplayConduit
      {
        #region -- Construction -----------------------------------------------

        //-- Solid Color/Material
        //--
        protected DisplayMaterial material;

        //-- Create Interals
        //--
        public Conduit( ) : base( )
        {
          material = new Rhino.Display.DisplayMaterial( System.Drawing.Color.White );
        }

        #endregion

        #region -- Bounding Box -----------------------------------------------

        //-- Bounding Box Calculation
        //--
        protected BoundingBox UpdateBounds( )
        {
          var bounds = new BoundingBox( );
          foreach( var body in bodies )
          {
            var box = new BoundingBox( body.bounds.Min, body.bounds.Max );
            box.Transform( body.transform );
            bounds.Union( box );
          }
          return bounds;
        }

        //-- Respond to View Changes
        //--
        protected override void CalculateBoundingBox( CalculateBoundingBoxEventArgs args )
        {
          args.IncludeBoundingBox( UpdateBounds( ) );
        }

        //-- Respond to Zoom Changes
        //--
        protected override void CalculateBoundingBoxZoomExtents( CalculateBoundingBoxEventArgs args )
        {
          args.IncludeBoundingBox( UpdateBounds( ) );
        }

        #endregion

        #region -- Dynamic Display --------------------------------------------

        protected override void PostDrawObjects( DrawEventArgs args )
        {
          Mat4D transform;
          foreach( var body in bodies )
          {
            //-- Get Current Transformation
            //--
            body.rigid.GetWorldTransform( out transform );
            body.transform = Convert( transform );

            //-- Apply Transformation Temporarily
            //--
            args.Display.PushModelTransform( body.transform );

              //-- Draw Solid Geometry
              //--
              args.Display.DrawBrepWires( body.solid, System.Drawing.Color.Black, 0 );
              args.Display.DrawBrepShaded( body.solid, material );

            args.Display.PopModelTransform( );
          }
        }

        #endregion
      }

      #endregion

      #region -- Solid Body ---------------------------------------------------

      //-- Bullet/Rhino Data
      //--
      public class Body
      {
        public RigidBody   rigid;              //-- Bullet Body  
        public Brep        solid;              //-- Rhino Solid   (can be any geometry really)
        public BoundingBox bounds;             //-- Bounding Box  (for clipping and zoom all)
        public Transform   transform;          //-- Tranformation (rhino to bullet xyz)
      }

      //-- Body Array
      //--
      public class Bodies : List<Body> { }

      #endregion

      #region -- Life Cycle ---------------------------------------------------

      private static bool initialized = false;

      //-- Create Simulation
      //--
      public static void StartUp( )
      {
        //-- Dispose Physics
        //--
        if( initialized )
        {
          initialized = false;
          ShutDown( );
        }

        //-- Create Bullet Data
        //--
        config   = new DefaultCollisionConfiguration( );
        dispatch = new CollisionDispatcher( config );
        phase    = new DbvtBroadphase( );
        solver   = new Solver( );
        world    = new DiscreteDynamicsWorld( dispatch, phase, solver, config );
        shapes   = new AlignedCollisionShapeArray( );
        bodies   = new Bodies( );

        //-- Create Rhino Data
        //--
        if( conduit == null )
        {
          conduit = new Conduit( );
        }
        conduit.Enabled = true;

        if( dialog == null )
        {
          dialog = new Dialog( );
        }
        if( !dialog.Visible )
        {
          dialog.Show( Rhino.RhinoApp.MainWindow( ) );
        }

        //-- Completed
        //--
        Rhino.RhinoDoc.ActiveDoc.Views.Redraw( );
        initialized = true;
      }

      //-- Reset Simulation
      //--
      public static void ShutDown( )
      {
        try
        {
          //-- Pause Animation Thread
          //--
          if( running )
          {
            pause.Reset( );
            running = false;
          }

          //-- Delete Rigid Bodies and Constraints
          //-- as in the folk-lift demo
          //--
          for( var index = world.CollisionObjectArray.Count - 1; index >= 0; index-- )
          {
            var obj = world.CollisionObjectArray[index];
            var body = obj as RigidBody;
            if( ( body != null ) && ( body.MotionState != null ) )
            {
              while( body.ConstraintRefCount != 0 )
              {
                var constraint = body.GetConstraintRef( 0 );
                world.RemoveConstraint( constraint );
                constraint.Dispose( );
              }
              if( body.MotionState != null )
              {
                body.MotionState.Dispose( );
              }
              world.RemoveRigidBody( body );
            }
            else
            {
              world.RemoveCollisionObject( obj );
            }
          }

          //-- Delete Constraints
          //--
          for( var index = world.NumConstraints - 1; index >= 0; index-- )
          {
            var constraint = world.GetConstraint( index );
            world.RemoveConstraint( constraint );
            if( !constraint.IsDisposed ) constraint.Dispose( );
          }

          //-- Delete Collision Shapes
          //--
          for( var index = shapes.Count - 1; index >= 0; index-- )
          {
            shapes[index].Dispose( );
          }
          shapes.Dispose( );

          //-- Delete Backwards
          //--
          world.Dispose( );
          solver.Dispose( );
          phase.Dispose( );
          dispatch.Dispose( );
          config.Dispose( );
        }

        //-- Crash Unavoidable
        //--
        catch( Exception except )
        {
          Plot( except.ToString( ) );
        }
      }

      #endregion

      #region -- Conversions --------------------------------------------------

      //-- Convert Bullet Matrix to Rhino Matrix
      //--
      public static Transform Convert( Mat4D matrix )
      {
        var result = new Transform( );

        //-- Set X-Axis
        //--
        result.M00 = matrix.M11;
        result.M10 = matrix.M12;
        result.M20 = matrix.M13;

        //-- Set Y-Axis
        //--
        result.M01 = matrix.M21;
        result.M11 = matrix.M22;
        result.M21 = matrix.M23;

        //-- Set Z-Axis
        //--
        result.M02 = matrix.M31;
        result.M12 = matrix.M32;
        result.M22 = matrix.M33;

        //-- Set Origin
        //--
        result.M03 = matrix.M41;
        result.M13 = matrix.M42;
        result.M23 = matrix.M43;
        result.M33 = 1.0;

        return result;
      }

      #endregion

      #region -- Demos --------------------------------------------------------

      //-- Build Geometry
      //--
      public static void Demo101( )
      {
        //-- Set Gravity
        //--
        world.Gravity = new Vec3D( 0, 0, -10 );

        //-- Create Ground 
        //--
        var body = new Body( );
        {
          //-- Define Infinite Plane
          //--
          var shape = new StaticPlaneShape( new Vec3D( 0, 0, 1 ), 0 );
          shapes.Add( shape );

          //-- Set Physics State / Bullet
          //-- Fixed bodies have zero mass and inertia
          //-- 
          var param = new RigidBodyConstructionInfo( 
            mass: 0.0f, motionState: new DefaultMotionState( Mat4D.Identity ),
            collisionShape: shape, localInertia: Vec3D.Zero );
          body.rigid = new RigidBody( param );
          param.Dispose( );

          world.AddRigidBody( body.rigid );

          //-- Set Visual State / Rhino
          //-- Create a very thin but wide finite box
          //--
          body.bounds = new BoundingBox( -50, -50, -0.01, 50, 50, 0 );
          body.solid  = body.bounds.ToBrep( );
        }
        bodies.Add( body );

        //-- Create 3D Grid of Boxes
        //--
        var random = new Random( );
        for( var y = 0; y < 4; y++ )
        {
          for( var x = 0; x < 4; x++ )
          {
            for( var z = 0; z < 4; z++ )
            {
              body = new Body( );
              {
                var noise = (float)( 0.2 * ( 1.0 - 2.0 * random.NextDouble( ) ) );

                //-- Position
                //--
                var px = x * 2.0f - 3.0f + noise;
                var py = y * 2.0f - 3.0f + noise;
                var pz = z * 2.0f + 9.0f + noise;
              
                //-- Cube Half Size
                //--
                var hx = 0.5f;
                var hy = 0.5f;
                var hz = 0.5f;

                //-- Collision Shape
                //--
                var shape = new BoxShape( hx, hy, hz );
                shapes.Add( shape );

                //-- Mass Properties
                //--
                var inertia = Vec3D.Zero;
                shape.CalculateLocalInertia( mass: 1.0f, inertia: out inertia );

                //-- Physics State
                //--
                var param = new RigidBodyConstructionInfo(
                  mass: 1.0f, motionState: new DefaultMotionState( Mat4D.Identity ),
                  collisionShape: shape, localInertia: inertia );
                body.rigid = new RigidBody( param );
                param.Dispose( );

                world.AddRigidBody( body.rigid );

                body.rigid.Translate( new Vec3D( px, py, pz ) );

                //-- Visual State
                //--
                body.bounds = new BoundingBox( -hx, -hy, -hz, hx, hy, hz );
                body.solid = body.bounds.ToBrep( );
              }
              bodies.Add( body );
            }
          }
        }
      }

      public static void Demo201( )
      {
        //-- Set Gravity
        //--
        world.Gravity = new Vec3D( 0, 0, -10 );

        //-- Create Sticks
        //--        
        for( var index = 0; index < 12; index++ )
        {
          var body = new Body( );

          //-- Collision Shape
          //--
          var shape = new BoxShape( 4, 1, 1 );
          shapes.Add( shape );

          //-- Setup Properties
          //--
          var inertia = new Vec3D( 0, 0, 0 );
          shape.CalculateLocalInertia( 1, out inertia );

          var info = new RigidBodyConstructionInfo( 
            1.0f, new DefaultMotionState( Mat4D.Identity ), shape, inertia );

          //-- Create Rigid Body
          //--
          body.rigid = new RigidBody( info );
          body.rigid.SetDamping( 0.5f, 0.5f );
          
          info.Dispose( );

          //-- Apply Transform
          //--
          var origin = new Vec3D( index * 5, 0, 0 );
          body.rigid.Translate( origin );
 
          world.AddRigidBody( body.rigid );
          
          //-- Create Visual
          //--
          body.bounds = new BoundingBox( -2, -0.5, -0.5, 2, 0.5, 0.5 );
          body.solid = body.bounds.ToBrep( );

          bodies.Add( body );
        }

        //-- Create Ball Joint
        //--
        world.AddConstraint( new Point2PointConstraint( bodies[0].rigid, new Vec3D( 0, 0, 0 ) ), false );
        for( var index = 1; index < bodies.Count; index++ )
        {
          world.AddConstraint( new Point2PointConstraint( 
            bodies[index-1].rigid,
            bodies[index-0].rigid,                                                                           
            new Vec3D(  3, 0, 0 ),                                                                           
            new Vec3D( -3, 0, 0 ) ), true );
        }
        world.AddConstraint( new Point2PointConstraint( bodies[bodies.Count-1].rigid, new Vec3D( 0, 0, 0 ) ), false );
      }      

      #endregion

      #region -- Threading ----------------------------------------------------

      public static void Update( )
      {
        if( !initialized ) return;
        try
        {
          world.StepSimulation( 20 );
          Rhino.RhinoDoc.ActiveDoc.Views.Redraw( );
        }
        catch { }
      }

      public static ManualResetEvent pause = new ManualResetEvent( true  );
      public static ManualResetEvent abort = new ManualResetEvent( false );
      public static bool running = false;

      public static void StartStop( )
      {
        if( thread == null )
        {
          thread = new Thread( ( ) => 
          {
            while( true )
            {
              pause.WaitOne( Timeout.Infinite );
              if( abort.WaitOne( 0 ) ) break;
              Update( );
            }          
          } );
          thread.Start( );          
        }

        if( running )
        {
          pause.Reset( );
        }
        else
        {
          pause.Set( );
        }

        running = !running;        
      }

      #endregion

      #region -- Dialog -------------------------------------------------------

      public class Dialog : Form
      {
        public Dialog( ) : base( )
        {
          Text = "Physics";
          Width = 200;
          Height =100;

          SuspendLayout( );

          var active = new Button( );
          active.Text = "Stopped";
          active.Click += ( sender, args ) =>
          {
            StartStop( );

            if( running )
            {
              active.Text = "Running";
            }
            else
            {
              active.Text = "Stopped";
            }
          };
          active.Dock = DockStyle.Fill;

          Controls.Add( active );

          ResumeLayout( );
        }
      }

      #endregion

    }

    //-- Rhino Command Invoked by Name
    //--
    public void physics101( )
    {
      try
      {
        Physics.StartUp( );
        Physics.Demo101( );
      }
      catch( Exception except )
      {
        Plot( except.ToString( ) );
      }
    }

    public void physics201( )
    {
      try
      {
        Physics.StartUp( );
        Physics.Demo201( );
      }
      catch( Exception except )
      {
        Plot( except.ToString( ) );
      }
    }
  }
}